home *** CD-ROM | disk | FTP | other *** search
/ CU Amiga Super CD-ROM 12 / CU Amiga Magazine's Super CD-ROM 12 (1997)(EMAP Images)(GB)[!][issue 1997-07].iso / CUCD / Sound / MusicIn / FFT_float.c < prev    next >
C/C++ Source or Header  |  1996-08-21  |  7KB  |  294 lines
  1. /*------------------------------------------------------------------------------
  2.  
  3.     File    :    FFT_float.c
  4.  
  5.     Author  :    Stéphane TAVENARD
  6.  
  7.     $VER:   FFT_float.c  0.0  (22/04/1995)
  8.  
  9.     (C) Copyright 1995-1995 Stéphane TAVENARD
  10.     All Rights Reserved
  11.  
  12.     #Rev|   Date   |              Comment
  13.     ----|----------|--------------------------------------------------------
  14.     0    |22/04/1995| Initial revision
  15.  
  16.     ------------------------------------------------------------------------
  17.  
  18.     FFT for floating point values
  19.  
  20. ------------------------------------------------------------------------------*/
  21.  
  22. #include <stdio.h>
  23. #include <math.h>
  24. #include "FFT_float_asm.h"
  25. #include "FFT_float.h"
  26.  
  27. #define FFT_ASM
  28.  
  29. static WORD INDEX[ FFT_RANGE_MAX ];
  30. static FLOAT COS[ FFT_RANGE_MAX/2 ];
  31. static FLOAT SIN[ FFT_RANGE_MAX/2 ];
  32.  
  33. static FLOAT temp_real[ FFT_RANGE_MAX ];
  34. static FLOAT temp_imag[ FFT_RANGE_MAX ];
  35.  
  36. static int range = 0;
  37. static int power = 0;
  38.  
  39. static void build_index( int power )
  40. {
  41.    WORD n, i, j, l, m;
  42.  
  43.    n = 1<<power;
  44.    for( i=0; i<n; i++ ) {
  45.       INDEX[ i ] = 0;
  46.       l = 1;
  47.       m = n;
  48.       for( j=0; j<power; j++ ) {
  49.      m = m>>1;
  50.      if( i & l ) INDEX[ i ] += m;
  51.      l = l<<1;
  52.       }
  53. //fprintf( stderr, "%04X -> %04X\n", i, INDEX[ i ] );
  54.    }
  55. }
  56.  
  57. static void build_sin_cos( int power )
  58. {
  59.    WORD i, n;
  60.    double p;
  61.  
  62. //fprintf( stderr, "Build sin/cos power=%ld\n", power );
  63.    n = 1<<power;
  64.    p = (2*PI)/(double)n;
  65.    n = n>>1;
  66.    for( i=0; i<n; i++ ) {
  67.       COS[ i ] = (FLOAT)cos( p * (double)i );
  68.       SIN[ i ] = (FLOAT)sin( p * (double)i );
  69.    }
  70. }
  71.  
  72. void FFT_FLOAT_forward( FLOAT *x_real, FLOAT *x_imag,
  73.             FLOAT *energy, FLOAT *phi, int n )
  74. {
  75.    WORD li, i;
  76.    register WORD d, j, l;
  77.    WORD *index;
  78.    FLOAT *real_ptr, *imag_ptr;
  79.    FLOAT *t_real_ptr, *t_imag_ptr;
  80.    FLOAT *en_ptr, *phi_ptr;
  81.    FLOAT real, imag;
  82.    FLOAT *cos_ptr, *sin_ptr;
  83.    FLOAT en;
  84.  
  85.    if( n > FFT_RANGE_MAX ) {
  86.       fprintf( stderr, "FFT_DOUBLE_forward: n is out of range (%ld>%ld)\n",
  87.                n, FFT_RANGE_MAX );
  88.       return;
  89.    }
  90.    if( n != range ) {
  91.       range = 1;
  92.       power = 0;
  93.       while( range < n ) {
  94.      range = range << 1;
  95.      power++;
  96.       }
  97.       if( range != n ) {
  98.      range = 0;
  99.      power = 0;
  100.      fprintf( stderr, "FFT_DOUBLE_forward: n is not a power of 2 (%ld)\n", n );
  101.      return;
  102.       }
  103.       build_index( power );
  104.       build_sin_cos( power );
  105.    }
  106.  
  107.    index = INDEX;
  108.    real_ptr = x_real;
  109.    imag_ptr = x_imag;
  110.    for( i=0; i<n; i++ ) {
  111.       j = *index++;
  112.       if( i < j ) {
  113.      real = x_real[ j ];
  114.      imag = x_imag[ j ];
  115.      x_real[ j ] = *real_ptr;
  116.      x_imag[ j ] = *imag_ptr;
  117.      *real_ptr++ = real;
  118.      *imag_ptr++ = imag;
  119.       }
  120.       else {
  121.      real_ptr++;
  122.      imag_ptr++;
  123.       }
  124.    }
  125.  
  126. #ifndef FFT_ASM
  127.    d = 1;
  128.    li = 1<<( power-1 );
  129.  
  130.    for( i=0; i<power-1; i++ ) {
  131.       real_ptr = x_real;
  132.       imag_ptr = x_imag;
  133.       t_real_ptr = &x_real[ d ];
  134.       t_imag_ptr = &x_imag[ d ];
  135.       for( j=0; j<li; j++ ) {
  136.      for( l=0; l<d; l++ ) {
  137.         real = *real_ptr + *t_real_ptr;
  138.         imag = *imag_ptr + *t_imag_ptr;
  139.         *t_real_ptr = *real_ptr - *t_real_ptr;
  140.         *t_imag_ptr = *imag_ptr - *t_imag_ptr;
  141.         t_real_ptr++;
  142.         t_imag_ptr++;
  143.         *real_ptr++ = real;
  144.         *imag_ptr++ = imag;
  145.      }
  146.      real_ptr += d;
  147.      imag_ptr += d;
  148.      t_real_ptr += d;
  149.      t_imag_ptr += d;
  150.       }
  151.       d = d<<1;
  152.       li = li>>1;
  153.       real_ptr = &x_real[ d ];
  154.       imag_ptr = &x_imag[ d ];
  155.       for( j=0; j<li; j++ ) {
  156.      cos_ptr = COS;
  157.      sin_ptr = SIN;
  158.      for( l=0; l<d; l++ ) {
  159.         real = (*real_ptr)*(*cos_ptr) + (*imag_ptr)*(*sin_ptr);
  160.         imag = (*imag_ptr)*(*cos_ptr) - (*real_ptr)*(*sin_ptr);
  161.         *real_ptr++ = real;
  162.         *imag_ptr++ = imag;
  163.         cos_ptr += li;
  164.         sin_ptr += li;
  165.      }
  166.      real_ptr += d;
  167.      imag_ptr += d;
  168.       }
  169.    }
  170.  
  171.    /* Optimized last loop */
  172.    real_ptr = x_real;
  173.    imag_ptr = x_imag;
  174.    t_real_ptr = &x_real[ d ];
  175.    t_imag_ptr = &x_imag[ d ];
  176.    for( j=0; j<d; j++ ) {
  177.       real = *real_ptr + *t_real_ptr;
  178.       imag = *imag_ptr + *t_imag_ptr;
  179.       *t_real_ptr = *real_ptr - *t_real_ptr;
  180.       *t_imag_ptr = *imag_ptr - *t_imag_ptr;
  181.       t_real_ptr++;
  182.       t_imag_ptr++;
  183.       *real_ptr++ = real;
  184.       *imag_ptr++ = imag;
  185.    }
  186. #else
  187.    ASM_FFT_main_loop( x_real, x_imag, COS, SIN, power );
  188. #endif
  189.  
  190. #ifndef FFT_ASM
  191.    real_ptr = x_real;
  192.    imag_ptr = x_imag;
  193.    en_ptr = energy;
  194.    phi_ptr = phi;
  195.    for( j=0; j<n; j++ ) {
  196.       real = *real_ptr++;
  197.       imag = *imag_ptr++;
  198.       en = real*real + imag*imag;
  199.       if( en == 0 ) {
  200.      *phi_ptr++ = 0;
  201.       }
  202.       else {
  203.      *phi_ptr++ = atan2( (double)imag, (double)real );
  204.       }
  205.       *en_ptr++ = en;
  206.    }
  207. #else
  208.    ASM_FFT_energy_phi( x_real, x_imag, energy, phi, n );
  209. #endif
  210. }
  211.  
  212. void FFT_FLOAT_reverse( FLOAT *x_real, FLOAT *x_imag,
  213.             FLOAT *energy, FLOAT *phi, int n )
  214. {
  215.    WORD li, i;
  216.    register WORD d, j, l;
  217.    FLOAT *out_real, *out_imag;
  218.    FLOAT en, ph;
  219.    if( n > FFT_RANGE_MAX ) {
  220.       fprintf( stderr, "FFT_DOUBLE_forward: n is out of range (%ld>%ld)\n",
  221.                n, FFT_RANGE_MAX );
  222.       return;
  223.    }
  224.    if( n != range ) {
  225.       range = 1;
  226.       power = 0;
  227.       while( range < n ) {
  228.      range = range << 1;
  229.      power++;
  230.       }
  231.       if( range != n ) {
  232.      range = 0;
  233.      power = 0;
  234.      fprintf( stderr, "FFT_DOUBLE_forward: n is not a power of 2 (%ld)\n", n );
  235.      return;
  236.       }
  237.       build_index( power );
  238.       build_sin_cos( power );
  239.    }
  240.  
  241.    out_real = energy;
  242.    out_imag = phi;
  243.  
  244.    for( i=0; i<n; i++ ) {
  245.       out_real[ i ] = x_real[ INDEX[ i ] ];
  246.       out_imag[ i ] = x_imag[ INDEX[ i ] ];
  247.    }
  248.  
  249.    d = 1;
  250.    li = 1<<( power - 2 );
  251.    for( i=0; i<power; i++ ) {
  252.       for( j=0; j<n; j++ ) {
  253.      if( j & d ) {
  254.         temp_real[ j ] = out_real[ j - d ] - out_real[ j ];
  255.         temp_imag[ j ] = out_imag[ j - d ] - out_imag[ j ];
  256.      }
  257.      else {
  258.         temp_real[ j ] = out_real[ j ] + out_real[ j + d ];
  259.         temp_imag[ j ] = out_imag[ j ] + out_imag[ j + d ];
  260.      }
  261.       }
  262.       d = d<<1;
  263.       l = 0;
  264.       for( j=0; j<n; j++ ) {
  265.      if( j & d ) {
  266.         out_real[ j ] = temp_real[ j ]*COS[ l ] - temp_imag[ j ]*SIN[ l ];
  267.         out_imag[ j ] = temp_imag[ j ]*COS[ l ] + temp_real[ j ]*SIN[ l ];
  268.         l += li;
  269.      }
  270.      else {
  271.         l = 0;
  272.         out_real[ j ] = temp_real[ j ];
  273.         out_imag[ j ] = temp_imag[ j ];
  274.      }
  275.       }
  276.       li = li>>1;
  277.    }
  278.    for( i=0; i<n; i++ ) {
  279.       x_real[ i ] = out_real[ i ]/n;
  280.       x_imag[ i ] = out_imag[ i ]/n;
  281.       en = x_real[ i ]*x_real[ i ] + x_imag[ i ]*x_imag[ i ];
  282.       if( en == 0 ) {
  283.      ph = 0;
  284.       }
  285.       else {
  286.      ph = atan2( (double)x_imag[ i ], (double)x_real[ i ] );
  287.       }
  288.       energy[ i ] = en;
  289.       phi[ i ] = ph;
  290.    }
  291. }
  292.  
  293.  
  294.